#include "component.h"
#include "device.h"

#define MSENSOR_LOG(format, ...)    OSAL_LOG(format, ##__VA_ARGS__)

#define EVENT_MSENSOR_PROC     0X00000001 //Msensor处理事件



#define IIC_VHW_MSENSOR        vIIC_1  //vhw接口
#define IIC_ADDR_MSENSOR       0X0C    //iic芯片地址

#define MSENSOR_REG_DEVICE_ID  0X00    //设备ID寄存器
#define MSENSOR_REG_ST1        0X10    //状态寄存器1 -- DRDY
#define MSENSOR_REG_HXL        0X11    //X轴数据寄存器L
#define MSENSOR_REG_HXH        0X12    //X轴数据寄存器H
#define MSENSOR_REG_HYL        0X13    //Y轴数据寄存器L
#define MSENSOR_REG_HYH        0X14    //Y轴数据寄存器H
#define MSENSOR_REG_HZL        0X15    //Z轴数据寄存器L
#define MSENSOR_REG_HZH        0X16    //Z轴数据寄存器H
#define MSENSOR_REG_TMPS       0X17    //
#define MSENSOR_REG_ST2        0X18    //状态寄存器1 -- HOFL[bit3]
#define MSENSOR_REG_CNTL2      0X31    //控制寄存器2 -- MODE[4:0]
#define MSENSOR_REG_CNTL3      0X32    //控制寄存器3 -- SRST




static ErrorStatus Msensor_Read(uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    if (Device_Write(IIC_VHW_MSENSOR, &reg_addr, 1, IIC_ADDR_MSENSOR) == 0)
    {
        return (Device_Read(IIC_VHW_MSENSOR, data, len, IIC_ADDR_MSENSOR) == 0) ? SUCCESS : ERROR;
    }
    return ERROR;
}

static ErrorStatus Msensor_Write(uint8_t reg_addr, uint8_t *data, uint8_t len)
{
    uint8_t buffer[10] = {0};

    buffer[0] = reg_addr;
    memcpy(&buffer[1], data, len);
    return (Device_Write(IIC_VHW_MSENSOR, buffer, len+1, IIC_ADDR_MSENSOR) == 0) ? SUCCESS : ERROR;
}

static void Msensor_Reset(void)
{
    uint8_t srst = 1;
    Msensor_Write(MSENSOR_REG_CNTL3, &srst, 1);
}

static void Msensor_SingleMeasurement(void)
{
    uint8_t mode = 0;

    /* 启动单次测量模式 */
    Msensor_Reset();//复位后延时100us
    // Msensor_Write(MSENSOR_REG_CNTL2, &mode, 1);
    OSAL_DelayUs(100);
    mode = 0x01;//0X10;
    Msensor_Write(MSENSOR_REG_CNTL2, &mode, 1);
}

static void Msensor_ReadData(void)
{
    uint8_t drdy = 0;
    
    Msensor_Read(MSENSOR_REG_ST1, &drdy, 1);
    if (drdy != 0)
    {
        uint8_t data[8] = {0};
        int16_t *p = (int16_t *)data;

        /* 读XYZ数据 */
        Msensor_Read(MSENSOR_REG_HXL, data, 8);

        float value[3];
        value[0] = p[0] * 0.15;
        value[1] = p[1] * 0.15;
        value[2] = p[2] * 0.15;

        float sum = abs(value[0]) + abs(value[1]) + abs(value[2]);

        MSENSOR_LOG("x:%-15f  y:%-15f  z:%-15f  SUM:%-15fuT\r\n", value[0], value[1], value[2], sum);

        //TODO 判断3轴的绝对值之和
        //饱和后自测一下？

        /* 启动新一轮测量 */
        Msensor_SingleMeasurement();
    }
    else
    {
        MSENSOR_LOG("Msensor Data not ready\r\n");
    }
}

/**
  * @brief  地磁传感器初始化
  *         
  * @note   
  */
static void Msensor_Init(void)
{
    static FlagStatus power_on = SET;

    if (power_on == SET)
    {
        power_on = RESET;

        /* 读设备ID */
        uint16_t id = 0;
        Msensor_Read(MSENSOR_REG_DEVICE_ID, (uint8_t*)&id, 2);
        MSENSOR_LOG("Msensor id:%04x\r\n", id);

        /* 启动单次转换 */
        Msensor_SingleMeasurement();
    }
}

/**
  * @brief  地磁传感器任务函数
  *
  * @note
  *         
  * @param  event：当前任务的所有事件
  *
  * @return 返回未处理的事件
  */
static uint32_t Msensor_Task(uint32_t event)
{
	/* 系统启动事件 */
	if (event & EVENT_SYS_START)
	{
		MSENSOR_LOG("Msensor task start\r\n");
		
        Msensor_Init();
        OSAL_EventRepeatCreate(COMP_MSENSOR, EVENT_MSENSOR_PROC, 300, EVT_PRIORITY_MEDIUM);
		return ( event ^ EVENT_SYS_START );
	}
	
	/* 系统休眠事件 */
	if (event & EVENT_SYS_SLEEP)
	{
		return ( event ^ EVENT_SYS_SLEEP );
	}

    if (event & EVENT_MSENSOR_PROC)
    {
        Msensor_ReadData();
        return ( event ^ EVENT_MSENSOR_PROC );
    }
	
	/* 系统中断事件 */
	if (event & EVENT_SYS_ISR)
	{
		return ( event ^ EVENT_SYS_ISR );
	}
	return 0;
}
COMPONENT_TASK_EXPORT(COMP_MSENSOR, Msensor_Task, 16);
